/*
 * MotorController.h
 *
 *  Created on: Nov 13, 2013
 *      Author: Kellen Carey
 */

#ifndef MOTORCONTROLLER_H_
#define MOTORCONTROLLER_H_

#define MAINFILE

#include <vector>
#include <ctime>
#include <iostream>
#include <fstream>
#include <cmath>

#include "MX28.h"
#include "MotionStatus.h"
#include "FSR.h"

#define NUM_MOTORS	12
#define NUM_LEG_MOTORS  12
#define NUM_ARM_MOTORS	6
#define TOTAL_MOTORS	24

#define MOTIONS_TOTAL 9
#define SPEED_CONSTANT	0.1285
#define INITIAL_SPEED	15

#define CHECK_POSITIONS		6
#define POSITION_TOLERANCE	25
//#define P_GAIN		28	// This is the address of the P value, DO NOT CHANGE

#define DEGREES_PER_POSITION	0.08789
#define RADIANS_PER_DEGREE		0.01745
#define P_FOR_PID	15	// Change this value to change the P value
#define NEW_RETURN_DELAY_TIME	10
#define STATUS_RETURN			1	// 0 returns nothing, 1 returns only read, 2 returns everything
#define LEG_CUTOFF				12


#define TORQUE_ENABLE			24
#define MOVING_SPEED			32
#define GOAL_ACCELERATION		73
#define PRESENT_LOAD			40
#define CW_LIMIT			6
#define CCW_LIMIT			8


#define DEFAULT_MOVING_SPEED	50
#define DEFAULT_ACCELERATION	15
#define LOW_ACCELERATION		15
#define GOAL_POSITION			30
#define PRESENT_POSITION		36
#define RETURN_DELAY_TIME		5
#define STATUS_RETURN_LEVEL		16
#define DEFAULT_MAX_TORQUE		1023
#define MAX_TORQUE				34

#define BYTE					1
#define WORD					2


using namespace Robot;

class MotorController {
private:
	static MotorController* m_UniqueInstance;

void initialize();

public:
	enum
	{
		ID_CM			= 200,
		ID_BROADCAST	= 254
	};
	enum
	{
		SUCCESS,
		TX_CORRUPT,
		TX_FAIL,
		RX_FAIL,
		RX_TIMEOUT,
		RX_CORRUPT
	};

	enum
	{
		INPUT_VOLTAGE   = 1,
		ANGLE_LIMIT     = 2,
		OVERHEATING     = 4,
		RANGE           = 8,
		CHECKSUM        = 16,
		OVERLOAD        = 32,
		INSTRUCTION     = 64
	};

	static MotorController* GetInstance() { return m_UniqueInstance; }

	MotorController();
	virtual ~MotorController();

	//TODO for cm730 transfer. For motion control
	void SyncWrite(int start_addr, int each_length, int number, int *pParam);

	void sendSyncWrite(std::vector<int>, int, int, std::vector<int>);
	void changePID(int, int);
	int readMotorPosition(int);
	int getMotorPositionReadWord(int);
	void setMotorLimits(int, int, int);	//sets CCW and Cw limits for motor x	void setTime(double); //takes new time input and passes it to editStep
	void setStatusReturnLevel(int);


};

#endif /* MOTORCONTROLLER_H_ */
